import numpy as np


def EstimatedConvergencePoint(a, b):
    # a是上一代粒子，b是moving vector
    I = np.identity(len(b[0]))
    b0 = np.zeros_like(b)
    sum1 = 0  # 式(11)第一项求和
    sum2 = 0  # 式(11)第二项求和
    for i in range(len(b)):
        b_norm = np.linalg.norm(b[i, :])
        if b_norm == 0:
            continue
        else:
            b0[i, :] = b[i, :] / b_norm ** 2
            H = I - np.dot(b0[i].reshape(-1, 1), b0[i].reshape(1, -1))
            sum1 += H
            sum2 += H.dot(a[i, :].reshape(-1, 1))
    if type(sum1) is int:
        return -1
    else:
        sum1 = np.array(sum1, dtype='float')
        sum2 = np.array(sum2, dtype='float')
        x = np.linalg.pinv(sum1).dot(sum2)
        return x
    